/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Occupancy grid based (ML) SLAM for J2B2 or similar ASRobo robots.
* 
* This is an implementation of an occupancy grid based SLAM algorithm. The algorithm is based on 
* Maximum likelihood (or more precicely maximum correlation) matching. The algorithm work "ok" if the 
* odometry is good enough. Should work well for small environments.
* 
* @author Jari
* TODO: 
- Correlation parameter inputs
- Sensor offset parameter

**/

#ifndef J2B2OccuSLAM_H_
#define J2B2OccuSLAM_H_

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "owndebug.h"
#include "ownutils.h"

#include "thread.hpp" ///< Thread base class

#include "CAHistogramECorrelationScanMatch.h" ///< Scan correlation
#include "TOccupancyGrid.h" ///< Occupancy grid class

#include "ASRoboClient.hpp" ///< Robo control client
#include "gimi.h"

///Some "nice" typecasts
#define TOGRIDSCAN(x) (*((Grid::TScan *) &x)

using namespace gim;

class J2B2OccuSLAM : public CThread{
		public:
				/**
				* Constructor
				* @param size_x the size of the grid in x-direction, in meters
				* @param size_y the size of the grid in y-direction, in meters
				* @param reso   Resolution. The size of one cell in meters
				* @param origin The position of the grid origin in the world frame of reference
				* @param cor_search_area The area used in searching the correlation (i.e. maximum odometry error)
				* @param cor_search_angle The angle used in searching the correlation (i.e. maximum odometry error)
				* @param number_of_scan_meas How many scans there is in the measurement
				*/
				J2B2OccuSLAM(float size_x, 
										 float size_y, 
										 float reso, 
										 Grid::TCoord2D origin,
										 float cor_search_area,
										 float cor_search_angle,
										 int number_of_scan_meas);
				
				~J2B2OccuSLAM();
				/**
				* Connect to the robot using initialized GIMI interface
				* @param *gim initialized gimi pointer
				* @param robot_name The name of the robot
				**/
				bool connectToRobot(gimi::GIMI *gim, std::string robot_name);
				/**
				* Connect to the robot by initializing own GIMI
				* @param hubhost name of the hubhost
				* @param hubport The hub port number_of_scan_meas
				* @param robot_name the name of the robot
				*/
				bool connectToRobot(std::string hubhost, int hubport, std::string robot_name);
				
				/**
				* The thread function - This is started IF ASRobo data source is
				* added with connectToRobot() call
				*/
				int ThreadFunction(const int aThreadNumber);
				
				void getPosition(float &x, float &y, float &a);
				
				void getMapDim(int &Nx, int &Ny){O->getDim(Nx,Ny);}
				/**
				* Pointer to map data.
				* NOTE: The data is changing all the time and this is not protected with mutex!!
				*/
				float *getMapDataPtr(){return O->getDataPtr();}
				
				/**
				* Transform world x,y to pixel coordinate
				*/
				Grid::TPixel toPixel(float x, float y){return O->toPixel(Grid::TCoord2D(x,y));}
				
				/**
				* Reset the map and pose
				**/
				void reset();
				/**
				* Set the SLAM algorithm to enabled or disabled 
				* by default the algorithm is enabled. 
				**/
				void setEnabled(bool isEnabled){isSLAMEnabled=isEnabled;}
				
				/**
				* Retruns the pointer to the occupancy grid.
				* NOTE: not protected by mutex!
				*/
				TOccupancyGrid *getOccupancyGridPtr(){return O;}
				
		private:
				bool isSLAMEnabled;
				TOccupancyGrid *O;              ///< Occupancy Grid
				CScanMatchInterface *gmatcher;  ///< Correlation algorithm
				ScanMatch::pose odo_meas_cur,odo_meas_ref;  ///< position of previous and curent laser mensurement
				ScanMatch::pose pos;            ///< The estimated position
				ScanMatch::scan meas,ref;           ///< Measurement(scan)
				ScanMatch::pose odo_cur,odo_ref,est; ///< Odometry measurements and estimated movement in between
				Grid::TScan grid_scan,virt;          ///< scans for grid 
				int numofscans;
				bool reference_set; 
				bool odo_set;
				ownMutexHandle dataMutex;            ///< mutex to protect the data
				float out_x,out_y,out_a;             ///< Output pose
				//////////////////////////////////////
				/// ASRobo client
				//////////////////////////////////////
				gimi::GIMI *gi;
				std::string robot; ///< the name of the robot
				CASRoboClient   *ASRoboClient;
				CPositionClient *PositionClient;
				CRangingClient  *LaserClient; ///< Pointer to ASRobo Ranging int
				
				/**
				* Initialize the robot control module parts
				*/
				bool initializeRobot();
				int filterScan(ScanMatch::scan &m, Grid::TScan &g);
				int filterScan(Grid::TScan &v,ScanMatch::scan &out);
				
				void setPosition(float &x, float &y, float &a);
};

#endif


